// PID control code
// Tim Hirzel
// December 2007

// This is a module that implements a PID control loop
// initialize it with 3 values: p,i,d
// and then tune the feedback loop with the setP etc funcs
//
// this was written based on a great PID by Tim Wescott:
// http://www.embedded.com/2000/0010/0010feat3.htm
//
//
// All code released under
// Creative Commons Attribution-Noncommercial-Share Alike 3.0 

#define WINDUP_GUARD_GAIN 100.0

float pTerm, iTerm, dTerm; 

int pgainAddress, igainAddress, dgainAddress;

float updatePID(float targetPosition, float currentPosition, float *lastPosition, float *iState)
{
  // these local variables can be factored out if memory is an issue, 
  // but they make it more readable
  double result;
  float error;
  float windupGaurd;

  // determine how badly we are doing
  error = targetPosition - currentPosition;

  // the pTerm is the view from now, the pgain judges 
  // how much we care about error we are this instant.
  pTerm = pgain * error;

  // iState keeps changing over time; it's 
  // overall "performance" over time, or accumulated error
  *iState += error;

  // to prevent the iTerm getting huge despite lots of 
  //  error, we use a "windup guard" 

  // not necessary, but this makes windup guard values 
  // relative to the current iGain
  windupGaurd = WINDUP_GUARD_GAIN / igain;  

  if (*iState > windupGaurd) 
    *iState = windupGaurd;
  else if (*iState < -windupGaurd) 
    *iState = -windupGaurd;
  iTerm = igain * (*iState);

  // the dTerm, the difference between the current
  //  and last reading, indicated the "speed," 
  // how quickly the reading is changing. (aka. Differential)
  dTerm = (dgain * (currentPosition - *lastPosition));
  *lastPosition = currentPosition;
  return  pTerm + iTerm - dTerm;
}
